/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.commands;

/**
 *
 * @author Jarrod Risley
 */
public class Track extends CommandBase {
    
    /*
     * Global Class Variables
     */
    private boolean isFinished = false;
    
    private static final double RATE_CUTOFF = 5;
    
    
    public Track() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        
        requires(kittyNet);
        requires(leftDrivetrain);
        requires(rightDrivetrain);
        requires(mainArm);
        requires(intake);
        requires(shooter);
        
    } // end Track Constructor

    // Called just before this Command runs the first time
    protected void initialize() {
        
    } // end initialize

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        
        System.out.println("Adjusting Y...");
        
        if (kittyNet.getYError() <= 0 && kittyNet.isConnected()) {
            
            System.out.print("Current Y Error is: " + kittyNet.getYError());
            
//            mainArm.setArmToCameraMode(true);
//            mainArm.setSetpoint(kittyNet.getYError());
        
            // Test the command before using this to end the command...
            
//        if (mainArm.getRateOfArm() < RATE_CUTOFF) { 
//            
//            isFinished = true;
//            
        } // end if
        
    } // end execute

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return isFinished;
    } // end isFinished

    // Called once after isFinished returns true
    protected void end() {
        mainArm.setArmToCameraMode(false);
    } // end end...lolz

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
        end();
    } // end interrupted
    
} // end Track
